#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <emmintrin.h>
#include <tmmintrin.h>

#ifdef ALIGN_DATA
#	define __aligned__ __attribute__((aligned(16)))
#else
#	define __aligned__
#endif

#define N  (1<<24)

uint32_t sse2_popcount3(uint8_t* buffer, int chunks16) {
	static uint8_t mask33[16] = {0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x33}; 
	static uint8_t mask55[16] = {0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55, 0x55}; 
	static uint8_t mask0f[16] = {0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f, 0x0f}; 

	uint32_t result = 0;
	int i, n, k;

#ifdef DEBUG
	assert(chunks % 4 == 0)
#ifndef ALIGN_DATA
	assert(false);
#endif
#endif

	__asm__ volatile ("movdqu (%%eax), %%xmm5" : : "a" (mask55));
	__asm__ volatile ("movdqu (%%eax), %%xmm6" : : "a" (mask33));
	__asm__ volatile ("movdqu (%%eax), %%xmm7" : : "a" (mask0f));
	__asm__ volatile ("pxor %xmm4, %xmm4");	// global accumulator

	i = 0;
	while (chunks16 > 0) {
#define MAX (7*4)
		if (chunks16 > MAX) {
			k = MAX;
			chunks16 -= MAX;
		}
		else {
			k = chunks16;
			chunks16 = 0;
		}
#undef MAX

		__asm__ volatile ("pxor %xmm3, %xmm3"); // xmm3 -- local accumulator
		for (n=0; n < k; n += 4) {
#define block(index) \
			__asm__ volatile( \
				"movdqa	  (%%eax), %%xmm0	\n" \
				"movdqa    %%xmm0, %%xmm1	\n" \
				"psrlw         $1, %%xmm1	\n" \
				"pand      %%xmm5, %%xmm0	\n" \
				"pand      %%xmm5, %%xmm1	\n" \
				"paddb     %%xmm1, %%xmm0	\n" \
				"movdqa    %%xmm0, %%xmm1	\n" \
				"psrlw         $2, %%xmm1	\n" \
				"pand      %%xmm6, %%xmm0	\n" \
				"pand      %%xmm6, %%xmm1	\n" \
				"paddb     %%xmm1, %%xmm0	\n" \
				"movdqa    %%xmm0, %%xmm1	\n" \
				"psrlw         $4, %%xmm1	\n" \
				"pand      %%xmm7, %%xmm0	\n" \
				"pand      %%xmm7, %%xmm1	\n" \
				"paddb     %%xmm1, %%xmm0	\n" \
				"paddb     %%xmm0, %%xmm3	\n"	\
				: : "a" (&buffer[index]));

			block(i + 0*16);
			block(i + 1*16);
			block(i + 2*16);
			block(i + 3*16);
			i += 4*16;
		}

		// update global accumulator (two 32-bits counters)
		__asm__ volatile (
			"pxor	%xmm0, %xmm0		\n"
			"psadbw	%xmm0, %xmm3		\n"
			"paddd	%xmm3, %xmm4		\n"
		);
	}

	// finally add together 32-bits counters stored in global accumulator
	__asm__ volatile (
		"movhlps   %%xmm4, %%xmm0	\n"
		"paddd     %%xmm4, %%xmm0	\n"
		"movd      %%xmm0, %%eax	\n"
		: "=a" (result)
	);

	return result;
}

void
panic (char *m)
{
  fprintf (stderr, "%s\n", m);
  exit (0);
}

int
main (int argc, char *argv[])
{
  unsigned int n, i, count = 0, total=0, *p;
  if (argc > 1)
    n = atoi (argv[1]);
  else
    n = N;

  if (posix_memalign ((void **) &p, 16, n * sizeof (unsigned int)) == -1)
    panic ("malloc");

  /* Random init the vector */
  for (i = 0; i < n; i++)
    p[i] = rand ();

  /* Accumulates popcount for all elements */
  //for (i = 0; i < n; i++)
  //  count += __builtin_popcount (p[i]);

  uint32_t count2 = sse2_popcount3((uint8_t*) p, n/4);
 

  printf ("%d\n", count2);
}
